#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <nav_msgs/msg/path.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2/utils.h>
#include <queue>
#include <unordered_set>
#include <unordered_map>
#include <mutex>
#include <cmath>
#include <functional>
#include <memory>
#include <algorithm>
#include "tf2/transform_datatypes.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "std_msgs/msg/u_int8.hpp"

// 网格坐标结构体
struct GridCoord
{
    int x, y;
    GridCoord(int x = 0, int y = 0) : x(x), y(y) {}

    bool operator==(const GridCoord &other) const
    {
        return x == other.x && y == other.y;
    }

    bool operator<(const GridCoord &other) const
    {
        return (x < other.x) || (x == other.x && y < other.y);
    }
};

// 哈希函数特化
namespace std
{
    template <>
    struct hash<GridCoord>
    {
        size_t operator()(const GridCoord &coord) const
        {
            return static_cast<size_t>(coord.x) * 131071 + static_cast<size_t>(coord.y);
        }
    };
}

// A*节点
struct AStarNode
{
    GridCoord coord;
    double g_cost; // 从起点到当前节点的代价
    double h_cost; // 启发式代价（到目标的估计）
    double f_cost() const { return g_cost + h_cost; }

    // 父节点指针
    std::shared_ptr<AStarNode> parent;

    AStarNode(GridCoord coord, double g, double h,
              std::shared_ptr<AStarNode> parent = nullptr)
        : coord(coord), g_cost(g), h_cost(h), parent(parent) {}
};

// 用于优先队列的比较函数
struct CompareAStarNode
{
    bool operator()(const std::shared_ptr<AStarNode> &a,
                    const std::shared_ptr<AStarNode> &b)
    {
        return a->f_cost() > b->f_cost();
    }
};

class PathPlanner : public rclcpp::Node
{
public:
    // 小船状态枚举
    typedef enum STATE {
        RELAYING = 0, // 继续前进
        STOPPING = 1, // 停止状态
        PLANNING = 2, // 规划状态
    } State;

PathPlanner(const double grid_size, const double goal_tolerance, const double stop_distance, const bool has_goal, const STATE current_state)
    : Node("path_planner"),
      grid_size_(grid_size),
      goal_tolerance_(goal_tolerance),
      stop_distance_(stop_distance),
      has_goal_(has_goal),
      current_state_(current_state)
    {
        // 里程计
        odom_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10,
            std::bind(&PathPlanner::odom_callback, this, std::placeholders::_1));

        // 订阅IMU
        imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
            "/imu", 10,
            std::bind(&PathPlanner::imuCallback, this, std::placeholders::_1));

        lidar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/lidar/scan", 10,
            std::bind(&PathPlanner::lidar_callback, this, std::placeholders::_1));

        radar_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
            "/radar/scan", 10,
            std::bind(&PathPlanner::radar_callback, this, std::placeholders::_1));

        // 目标点订阅器
        goal_sub_ = this->create_subscription<geometry_msgs::msg::PointStamped>(
            "/goal_pose", 10,
            std::bind(&PathPlanner::goal_callback, this, std::placeholders::_1));

        // 路径发布器
        path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/astar_planning_path", 10);

        // 状态发布器
        state_pub_ = this->create_publisher<std_msgs::msg::UInt8>("/boat_state", 10);

        // 障碍物发布器
        obstacles_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/obstacles_markers", 10);

        // 路径规划定时器
        planning_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&PathPlanner::planning_callback, this));

        // 独立障碍物发布定时器
        obstacle_pub_timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500),
            std::bind(&PathPlanner::obstacle_pub_callback, this));

        RCLCPP_INFO(this->get_logger(), "Path planner initialized. Waiting for goal...");
    }

private:
    // 传感器回调函数
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_position_ = msg->pose.pose.position;
        // current_yaw_ = tf2::getYaw(msg->pose.pose.orientation);
    }

    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr msg) 
    {
        // 提取四元数
        tf2::Quaternion q(
            msg->orientation.x,
            msg->orientation.y,
            msg->orientation.z,
            msg->orientation.w
        );
        // 转换为欧拉角
        tf2::Matrix3x3 m(q);
        double roll, pitch, yaw;
        m.getRPY(roll, pitch, yaw);  // 航向角存储在 yaw 变量中
        std::lock_guard<std::mutex> lock(data_mutex_);
        current_yaw_ = yaw; // 航向角，逆时针逆时针为正 [-π , π]
        // 打印当前航向角 角度值
        // RCLCPP_INFO(this->get_logger(), "Current yaw: %f", current_yaw_ * 180.0 / M_PI);
    }

    void lidar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        latest_lidar_scan_ = msg;
    }

    void radar_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        latest_radar_scan_ = msg;
    }

    // 目标点回调函数
    void goal_callback(const geometry_msgs::msg::PointStamped::SharedPtr msg)
    {
        std::lock_guard<std::mutex> lock(data_mutex_);

        // 清除旧目标状态
        has_goal_ = false;
        publish_empty_path();

        // 更新目标点位置
        goal_position_.x = msg->point.x;
        goal_position_.y = msg->point.y;

        // 检查目标点是否在障碍物上（使用最新障碍物数据）
        GridCoord goal_grid = world_to_grid(goal_position_.x, goal_position_.y);
        if (obstacles_.find(goal_grid) != obstacles_.end())
        {
            RCLCPP_WARN(this->get_logger(),
                        "Warning: The goal (%.2f, %.2f) is in an obstacle grid (cell: [%d, %d])!",
                        goal_position_.x, goal_position_.y, goal_grid.x, goal_grid.y);
        }
        else
        {
            // 标记有目标点
            has_goal_ = true;
            current_state_ = PLANNING; // 接收到新目标时进入规划状态
            RCLCPP_INFO(this->get_logger(), "New goal received: (%.2f, %.2f)", goal_position_.x, goal_position_.y);
        }
    }

    void publish_empty_path()
    {
        nav_msgs::msg::Path empty_path;
        empty_path.header.stamp = this->now();
        empty_path.header.frame_id = "odom";
        path_pub_->publish(empty_path);
        RCLCPP_DEBUG(this->get_logger(), "Published empty path to clear previous path");
    }

    // 发布当前状态
    void publish_state()
    {
        std_msgs::msg::UInt8 state_msg;
        state_msg.data = static_cast<uint8_t>(current_state_);
        state_pub_->publish(state_msg);
    }

    // 独立障碍物发布回调
    void obstacle_pub_callback()
    {
        std::lock_guard<std::mutex> lock(data_mutex_);
        update_obstacles();
        publish_obstacles_markers();
    }

    // 从扫描数据更新障碍物
    void update_obstacles()
    {
        // 清除旧障碍物
        obstacles_.clear();
        obstacle_points_.clear();

        // 处理最新激光数据
        if (latest_lidar_scan_)
        {
            update_obstacles_from_scan(latest_lidar_scan_);
        }

        // 处理最新雷达数据
        if (latest_radar_scan_)
        {
            update_obstacles_from_scan(latest_radar_scan_);
        }
    }

    // 从激光扫描中更新障碍物
    void update_obstacles_from_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan)
    {
        std::map<GridCoord, unsigned int> obstacle_counts;
        
        for (size_t i = 0; i < scan->ranges.size(); ++i)
        {
            if (std::isinf(scan->ranges[i]) || std::isnan(scan->ranges[i]))
            {
                continue;
            }

            double angle = scan->angle_min + i * scan->angle_increment;
            double global_angle = angle + current_yaw_;

            double obs_x = current_position_.x + scan->ranges[i] * cos(global_angle);
            double obs_y = current_position_.y + scan->ranges[i] * sin(global_angle);

            // 保存原始障碍物点
            geometry_msgs::msg::Point obs_point;
            obs_point.x = obs_x;
            obs_point.y = obs_y;
            obstacle_points_.push_back(obs_point);

            // 转换为网格坐标
            GridCoord obs_cell = world_to_grid(obs_x, obs_y);
            obstacle_counts[obs_cell]++;
        }
        
        for (const auto &[obs_cell, count] : obstacle_counts)
        {
            if (count >= threshold_count_)
            {
                obstacles_.insert(obs_cell);
            }
        }
    }

    // 发布障碍物标记
    void publish_obstacles_markers()
    {
        visualization_msgs::msg::MarkerArray marker_array;

        // 删除之前的所有标记
        visualization_msgs::msg::Marker delete_all_marker;
        delete_all_marker.header.frame_id = "odom";
        delete_all_marker.header.stamp = this->now();
        delete_all_marker.action = visualization_msgs::msg::Marker::DELETEALL;
        marker_array.markers.push_back(delete_all_marker);

        // 为每个障碍物创建标记
        int marker_id = 0;
        for (const auto &obstacle : obstacles_)
        {
            visualization_msgs::msg::Marker marker;
            marker.header.frame_id = "odom";
            marker.header.stamp = this->now();
            marker.ns = "obstacles";
            marker.id = marker_id++;
            marker.type = visualization_msgs::msg::Marker::CUBE;
            marker.action = visualization_msgs::msg::Marker::ADD;

            // 网格中心的世界坐标
            auto [world_x, world_y] = grid_to_world(obstacle.x, obstacle.y);
            marker.pose.position.x = world_x;
            marker.pose.position.y = world_y;
            marker.pose.position.z = 0.0;
            marker.pose.orientation.w = 1.0; // 无旋转

            // 设置立方体尺寸
            marker.scale.x = grid_size_ * 0.8;
            marker.scale.y = grid_size_ * 0.8;
            marker.scale.z = 0.3; // 高度

            // 设置颜色
            marker.color.r = 1.0;
            marker.color.g = 1.0;
            marker.color.b = 0.0;
            marker.color.a = 0.5; // 半透明

            // 生命周期
            marker.lifetime = rclcpp::Duration::from_seconds(0.5);

            marker_array.markers.push_back(marker);
        }

        obstacles_pub_->publish(marker_array);
        RCLCPP_DEBUG(this->get_logger(), "Published %zu obstacle markers", obstacles_.size());
    }

    // 更新小船状态
    void update_boat_state()
    {
        // 优先级1：没有检测到危险点
        if (obstacle_points_.empty()) {
            current_state_ = RELAYING;
            RCLCPP_INFO(this->get_logger(), "No obstacles detected. State: RELAYING");
            return;
        }

        // 计算所有障碍物点到小船的距离
        std::vector<double> distances;
        for (const auto& point : obstacle_points_) {
            double dist = distance_between_points(current_position_, point);
            distances.push_back(dist);
        }

        // 排序获取最近障碍物
        std::sort(distances.begin(), distances.end());
        double closest_obstacle_dist = distances.front();

        // 优先级2：危险点过远
        if (closest_obstacle_dist > 2 * stop_distance_) {
            current_state_ = RELAYING;
            RCLCPP_INFO(this->get_logger(), "Obstacles too far (%.2fm). State: RELAYING", closest_obstacle_dist);
            return;
        }

        // 优先级3：危险点过近
        if (closest_obstacle_dist < stop_distance_) {
            current_state_ = STOPPING;
            RCLCPP_WARN(this->get_logger(), "Obstacle too close (%.2fm)! State: STOPPING", closest_obstacle_dist);
            return;
        }

        // 计算障碍物到目标的距离
        std::vector<double> goal_distances;
        for (const auto& point : obstacle_points_) {
            double dist = distance_between_points(goal_position_, point);
            goal_distances.push_back(dist);
        }

        // 排序获取最近目标障碍物
        std::sort(goal_distances.begin(), goal_distances.end());
        double closest_goal_obstacle = goal_distances.front();

        // 优先级4：危险点靠近终点
        if (closest_goal_obstacle < stop_distance_) {
            current_state_ = RELAYING;
            RCLCPP_INFO(this->get_logger(), "Obstacle near goal (%.2fm). State: RELAYING", closest_goal_obstacle);
            return;
        }

        // 默认状态：需要规划
        current_state_ = PLANNING;
        RCLCPP_INFO(this->get_logger(), "Planning required. State: PLANNING");
    }

    // 路径规划回调
    void planning_callback()
    {
        std::lock_guard<std::mutex> lock(data_mutex_);

        if (!has_goal_) // 如果没有目标点，则跳过规划
        {
            return;
        }
        update_obstacles(); // 更新障碍物数据
        update_boat_state(); // 更新小船状态
        publish_state(); // 发布当前状态

        // 处理STOPPING状态
        if (current_state_ == STOPPING) {
            publish_empty_path();
            RCLCPP_WARN(this->get_logger(), "Stopping due to nearby obstacle!");
            return;
        }

        // 检查是否到达目标
        if (distance_between_points(current_position_, goal_position_) < goal_tolerance_) {
            RCLCPP_INFO(this->get_logger(), "Goal reached!");
            has_goal_ = false;
            current_state_ = RELAYING;
            publish_state();
            publish_empty_path();
            return;
        }

        // 获取当前位置的网格坐标
        GridCoord start = world_to_grid(current_position_.x, current_position_.y);
        GridCoord goal = world_to_grid(goal_position_.x, goal_position_.y);

        // 检查目标点是否在障碍物上
        if (obstacles_.find(goal) != obstacles_.end()) {
            RCLCPP_WARN(this->get_logger(), "Goal is in obstacle! Clearing path.");
            has_goal_ = false;
            publish_empty_path();
            return;
        }

        // 执行A*路径规划
        std::vector<GridCoord> grid_path = a_star_planning(start, goal);

        if (!grid_path.empty()) {
            publish_path(grid_path);
        } else {
            RCLCPP_ERROR(this->get_logger(), "No valid path found!");
            publish_empty_path();
        }
    }

    // A*路径规划算法
    std::vector<GridCoord> a_star_planning(const GridCoord &start, const GridCoord &goal)
    {
        // 优先队列（最小堆）
        using NodePtr = std::shared_ptr<AStarNode>;
        std::priority_queue<NodePtr, std::vector<NodePtr>, CompareAStarNode> open_set;

        // 节点存储（避免重复创建）
        std::unordered_map<GridCoord, NodePtr> node_map;

        // 起点初始化
        auto start_node = std::make_shared<AStarNode>(
            start, 0, distance_between_coords(start, goal));
        open_set.push(start_node);
        node_map[start] = start_node;

        while (!open_set.empty())
        {
            // 获取f_cost最小的节点
            NodePtr current = open_set.top();
            open_set.pop();

            // 到达目标点
            if (current->coord == goal)
            {
                return reconstruct_path(current);
            }

            // 检查相邻网格 (8个方向)
            for (int dx = -1; dx <= 1; ++dx)
            {
                for (int dy = -1; dy <= 1; ++dy)
                {
                    if (dx == 0 && dy == 0)
                        continue;

                    GridCoord neighbor_coord(
                        current->coord.x + dx,
                        current->coord.y + dy);

                    // 跳过障碍物
                    if (obstacles_.find(neighbor_coord) != obstacles_.end())
                    {
                        continue;
                    }

                    // 计算移动成本（对角线成本更高）
                    double move_cost = (dx != 0 && dy != 0) ? 1.414 : 1.0;
                    double tentative_g = current->g_cost + move_cost;

                    // 检查节点是否已存在
                    auto it = node_map.find(neighbor_coord);
                    bool is_new_node = (it == node_map.end());
                    bool is_better_path = false;

                    if (!is_new_node)
                    {
                        // 如果找到更优路径
                        is_better_path = tentative_g < it->second->g_cost;
                    }

                    if (is_new_node || is_better_path)
                    {
                        // 创建新节点或更新现有节点
                        double h_cost = distance_between_coords(neighbor_coord, goal);
                        auto neighbor_node = std::make_shared<AStarNode>(
                            neighbor_coord, tentative_g, h_cost, current);

                        // 更新节点映射
                        node_map[neighbor_coord] = neighbor_node;

                        // 新节点加入开放集
                        open_set.push(neighbor_node);
                    }
                }
            }
        }

        return {}; // 未找到路径
    }

    // 重构路径
    std::vector<GridCoord> reconstruct_path(std::shared_ptr<AStarNode> goal_node)
    {
        std::vector<GridCoord> path;
        auto current = goal_node;

        while (current != nullptr)
        {
            path.push_back(current->coord);
            current = current->parent;
        }

        std::reverse(path.begin(), path.end());
        return path;
    }

    // 发布完整路径
    void publish_path(const std::vector<GridCoord> &grid_path)
    {
        nav_msgs::msg::Path path_msg;
        path_msg.header.stamp = this->now();
        path_msg.header.frame_id = "odom";

        for (size_t i = 0; i < grid_path.size(); ++i)
        {
            const auto &coord = grid_path[i];
            auto [world_x, world_y] = grid_to_world(coord.x, coord.y);

            geometry_msgs::msg::PoseStamped pose;
            pose.header = path_msg.header;
            pose.pose.position.x = world_x;
            pose.pose.position.y = world_y;
            pose.pose.position.z = 0.0;
            pose.pose.orientation.w = 1.0;

            path_msg.poses.push_back(pose);
        }

        path_pub_->publish(path_msg);
        RCLCPP_INFO(this->get_logger(), "Published path with %zu points", grid_path.size());
    }

    // 世界坐标转网格坐标
    GridCoord world_to_grid(double x, double y) const
    {
        return GridCoord(
            static_cast<int>(std::round(x / grid_size_)),
            static_cast<int>(std::round(y / grid_size_)));
    }

    // 网格坐标转世界坐标
    std::pair<double, double> grid_to_world(int grid_x, int grid_y) const
    {
        return {grid_x * grid_size_, grid_y * grid_size_};
    }

    // 计算两个网格坐标之间的距离
    double distance_between_coords(const GridCoord &a, const GridCoord &b) const
    {
        int dx = a.x - b.x;
        int dy = a.y - b.y;
        return std::sqrt(dx * dx + dy * dy) * grid_size_;
    }

    // 计算两个世界点之间的距离
    double distance_between_points(const geometry_msgs::msg::Point &a,
                                   const geometry_msgs::msg::Point &b) const
    {
        return std::hypot(a.x - b.x, a.y - b.y);
    }

    const double grid_size_; // 网格大小
    const double goal_tolerance_; // 目标点容忍度
    const double stop_distance_;  // 停止距离阈值
    int threshold_count_ = 3;
    State current_state_; // 小船状态

    // 数据存储
    geometry_msgs::msg::Point current_position_;
    geometry_msgs::msg::Point goal_position_;
    double current_yaw_ = 0.0; // 当前机器人朝向
    std::unordered_set<GridCoord> obstacles_;
    std::vector<geometry_msgs::msg::Point> obstacle_points_; // 原始障碍物点
    bool has_goal_; // 是否有目标点标志

    // 存储最新传感器数据
    sensor_msgs::msg::LaserScan::SharedPtr latest_lidar_scan_ = nullptr;
    sensor_msgs::msg::LaserScan::SharedPtr latest_radar_scan_ = nullptr;

    // ROS2 订阅和发布
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr lidar_sub_;
    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr radar_sub_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub_;
    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr obstacles_pub_;
    rclcpp::Publisher<std_msgs::msg::UInt8>::SharedPtr state_pub_; // 状态发布器
    rclcpp::TimerBase::SharedPtr planning_timer_;
    rclcpp::Subscription<geometry_msgs::msg::PointStamped>::SharedPtr goal_sub_;

    // 独立障碍物发布定时器
    rclcpp::TimerBase::SharedPtr obstacle_pub_timer_;
    // 线程安全
    std::mutex data_mutex_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    double grid_size = 1.0;
    double goal_tolerance = 0.3;
    double stop_distance = 1.0;
    bool has_goal = false; // 是否有目标点标志
    PathPlanner::State current_state = PathPlanner::State::RELAYING;
    auto node = std::make_shared<PathPlanner>(grid_size, goal_tolerance, stop_distance, has_goal, current_state);
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}